#include "Wire.h"



TwoWire::TwoWire()
{
    _i2c.sda = digitalPinToPinName(SDA);
    _i2c.scl = digitalPinToPinName(SCL);
}

TwoWire::TwoWire(uint8_t sda, uint8_t scl)
{
    _i2c.sda = digitalPinToPinName(sda);
    _i2c.scl = digitalPinToPinName(scl);
}

void TwoWire::begin(bool generalCall)
{
    begin(MASTER_ADDRESS, generalCall);
}

void TwoWire::begin(uint8_t sda, uint8_t scl)
{
    _i2c.sda = digitalPinToPinName(sda);
    _i2c.scl = digitalPinToPinName(scl);
    begin();
}

void TwoWire::begin(uint8_t address, bool generalCall)
{
    rxBufferIndex = 0;
    rxBufferLength = 0;
    rxBuffer = nullptr;
    rxBufferAllocated = 0;

    resetRxBuffer();

    txBufferIndex = 0;
    txBufferLength = 0;
    txAddress = 0;
    txBuffer = nullptr;
    txBufferAllocated = 0;
    resetTxBuffer();

    _i2c.__this = (void *)this;
    user_onRequest = NULL;
    transmitting = 0;

    ownAddress = address << 1;

    _i2c.isMaster = (address == MASTER_ADDRESS) ? 1 : 0;

    _i2c.generalCall = (generalCall == true) ? 1 : 0;

    i2c_custom_init(&_i2c, 100000, I2C_AcknowledgedAddress_7bit, ownAddress);

    if (_i2c.isMaster == 0)
    {
        // i2c_attachSlaveTxEvent(&_i2c, reinterpret_cast<void(*)(i2c_t*)>(&TwoWire::onRequestService));
        // i2c_attachSlaveRxEvent(&_i2c, reinterpret_cast<void(*)(i2c_t*, uint8_t*, int)>(&TwoWire::onReceiveService));

        i2c_attachSlaveTxEvent(&_i2c, onRequestService);
        i2c_attachSlaveRxEvent(&_i2c, onReceiveService);
    }
}

void TwoWire::begin(int address, bool generalCall)
{
    begin((uint8_t)address, generalCall);
}

void TwoWire::end()
{
    i2c_deinit(&_i2c);
    free(txBuffer);
    txBuffer = nullptr;
    txBufferAllocated = 0;
    free(rxBuffer);
    rxBuffer = nullptr;
    rxBufferAllocated = 0;
}

void TwoWire::setClock(uint32_t frequency)
{
    i2c_setTiming(&_i2c, frequency);
}

void TwoWire::beginTransmission(uint8_t address)
{
    // indicate that we are transmitting
    transmitting = 1;
    // set address of targeted slave
    txAddress = address << 1;
    // reset tx buffer iterator vars
    txBufferIndex = 0;
    txBufferLength = 0;
}

void TwoWire::beginTransmission(int address)
{
    beginTransmission((uint8_t)address);
}

uint8_t TwoWire::endTransmission(void)
{
    return endTransmission((uint8_t) true);
}

uint8_t TwoWire::endTransmission(uint8_t sendStop)
{
#if !defined(I2C_OTHER_FRAME)
    UNUSED(sendStop);
#endif
    int8_t ret = 4;
    // check transfer options and store it in the I2C handle
#if defined(I2C_OTHER_FRAME)
    if (sendStop == 0)
    {
        _i2c.handle.XferOptions = I2C_OTHER_FRAME;
    }
    else
    {
        _i2c.handle.XferOptions = I2C_OTHER_AND_LAST_FRAME;
    }
#endif

    if (_i2c.isMaster == 1)
    {
        // transmit buffer (blocking)
        switch (i2c_master_write(&_i2c, txAddress, txBuffer, txBufferLength))
        {
        case I2C_OK:
            ret = 0; // Success
            break;
        case I2C_DATA_TOO_LONG:
            ret = 1;
            break;
        case I2C_NACK_ADDR:
            ret = 2;
            break;
        case I2C_NACK_DATA:
            ret = 3;
            break;
        case I2C_TIMEOUT:
        case I2C_BUSY:
        case I2C_ERROR:
        default:
            ret = 4;
            break;
        }

        // reset Tx buffer
        resetTxBuffer();

        // reset tx buffer iterator vars
        txBufferIndex = 0;
        txBufferLength = 0;

        // indicate that we are done transmitting
        transmitting = 0;
    }
    return ret;
}

uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity)
{
    return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t) true);
}

uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop)
{
    return requestFrom((uint8_t)address, (uint8_t)quantity, (uint32_t)0, (uint8_t)0, (uint8_t)sendStop);
}

uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
{
#if !defined(I2C_OTHER_FRAME)
    UNUSED(sendStop);
#endif
    uint8_t read = 0;

    if (_i2c.isMaster == 1)
    {
        allocateRxBuffer(quantity);
        // error if no memory block available to allocate the buffer
        if (rxBuffer == nullptr)
        {
            setWriteError();
        }
        else
        {

            if (isize > 0)
            {
                // send internal address; this mode allows sending a repeated start to access
                // some devices' internal registers. This function is executed by the hardware
                // TWI module on other processors (for example Due's TWI_IADR and TWI_MMR registers)

                beginTransmission(address);

                // the maximum size of internal address is 3 bytes
                if (isize > 3)
                {
                    isize = 3;
                }

                // write internal register address - most significant byte first
                while (isize-- > 0)
                {
                    write((uint8_t)(iaddress >> (isize * 8)));
                }
                endTransmission(false);
            }

            // perform blocking read into buffer
#if defined(I2C_OTHER_FRAME)
            if (sendStop == 0)
            {
                _i2c.handle.XferOptions = I2C_OTHER_FRAME;
            }
            else
            {
                _i2c.handle.XferOptions = I2C_OTHER_AND_LAST_FRAME;
            }
#endif

            if (I2C_OK == i2c_master_read(&_i2c, address << 1, rxBuffer, quantity))
            {
                read = quantity;
            }

            // set rx buffer iterator vars
            rxBufferIndex = 0;
            rxBufferLength = read;
        }
    }
    return read;
}

uint8_t TwoWire::requestFrom(int address, int quantity)
{
    return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t) true);
}

uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop)
{
    return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)sendStop);
}

size_t TwoWire::write(uint8_t data)
{
    size_t ret = 1;
    if (transmitting)
    {
        // in master transmitter mode
        allocateTxBuffer(txBufferLength + 1);
        // error if no memory block available to allocate the buffer
        if (txBuffer == nullptr)
        {
            setWriteError();
            ret = 0;
        }
        else
        {
            // put byte in tx buffer
            txBuffer[txBufferIndex] = data;
            ++txBufferIndex;
            // update amount in buffer
            txBufferLength = txBufferIndex;
        }
    }
    else
    {
        // in slave send mode
        // reply to master
        if (i2c_slave_write_IT(&_i2c, &data, 1) != I2C_OK)
        {
            ret = 0;
        }
    }
    return ret;
}

size_t TwoWire::write(const uint8_t *data, size_t quantity)
{
    size_t ret = quantity;

    if (transmitting)
    {
        // in master transmitter mode
        allocateTxBuffer(txBufferLength + quantity);
        // error if no memory block available to allocate the buffer
        if (txBuffer == nullptr)
        {
            setWriteError();
            ret = 0;
        }
        else
        {
            // put bytes in tx buffer
            memcpy(&(txBuffer[txBufferIndex]), data, quantity);
            txBufferIndex = txBufferIndex + quantity;
            // update amount in buffer
            txBufferLength = txBufferIndex;
        }
    }
    else
    {
        // in slave send mode
        // reply to master
        if (i2c_slave_write_IT(&_i2c, (uint8_t *)data, quantity) != I2C_OK)
        {
            ret = 0;
        }
    }
    return ret;
}

int TwoWire::available(void)
{
    return rxBufferLength - rxBufferIndex;
}

int TwoWire::read(void)
{
    int value = -1;

    // get each successive byte on each call
    if (rxBufferIndex < rxBufferLength)
    {
        value = rxBuffer[rxBufferIndex];
        ++rxBufferIndex;

        /* Commented as not I think it is not useful
         * but kept to show that it is possible to
         * reset rx buffer when no more data available */
        /*if(rxBufferIndex == rxBufferLength) {
          resetRxBuffer();
        }*/
    }
    return value;
}

int TwoWire::peek(void)
{
    int value = -1;

    if (rxBufferIndex < rxBufferLength)
    {
        value = rxBuffer[rxBufferIndex];
    }
    return value;
}

void TwoWire::flush(void)
{
    rxBufferIndex = 0;
    rxBufferLength = 0;
    resetRxBuffer();
    txBufferIndex = 0;
    txBufferLength = 0;
    resetTxBuffer();
}

void TwoWire::onReceive(void (*function)(int))
{
    user_onReceive = function;
}

void TwoWire::onRequest(void (*function)(void))
{
    user_onRequest = function;
}

////////////////////////////////////////////////////////////////////

inline void TwoWire::allocateRxBuffer(size_t length)
{
    if (rxBufferAllocated < length)
    {
        // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
        if (length < BUFFER_LENGTH)
        {
            length = BUFFER_LENGTH;
        }
        uint8_t *tmp = (uint8_t *)realloc(rxBuffer, length * sizeof(uint8_t));
        if (tmp != nullptr)
        {
            rxBuffer = tmp;
            rxBufferAllocated = length;
        }
        else
        {
            _Error_Handler("No enough memory! (%i)\n", length);
        }
    }
}

inline void TwoWire::allocateTxBuffer(size_t length)
{
    if (txBufferAllocated < length)
    {
        // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
        if (length < BUFFER_LENGTH)
        {
            length = BUFFER_LENGTH;
        }
        uint8_t *tmp = (uint8_t *)realloc(txBuffer, length * sizeof(uint8_t));
        if (tmp != nullptr)
        {
            txBuffer = tmp;
            txBufferAllocated = length;
        }
        else
        {
            _Error_Handler("No enough memory! (%i)\n", length);
        }
    }
}

inline void TwoWire::resetRxBuffer(void)
{
    if (rxBuffer != nullptr)
    {
        memset(rxBuffer, 0, rxBufferAllocated);
    }
}

inline void TwoWire::resetTxBuffer(void)
{
    if (txBuffer != nullptr)
    {
        memset(txBuffer, 0, txBufferAllocated);
    }
}

void TwoWire::onRequestService(i2c_t *obj)
{
    TwoWire *TW = (TwoWire *)(obj->__this);

    // don't bother if user hasn't registered a callback
    if (TW->user_onRequest)
    {
        // reset tx buffer iterator vars
        // !!! this will kill any pending pre-master sendTo() activity
        TW->txBufferIndex = 0;
        TW->txBufferLength = 0;
        // alert user program
        TW->user_onRequest();
    }
}

void TwoWire::onReceiveService(i2c_t *obj)
{
    uint8_t *inBytes = (uint8_t *)obj->i2cTxRxBuffer;
    int numBytes = obj->slaveRxNbData;

    TwoWire *TW = (TwoWire *)(obj->__this);

    // don't bother if user hasn't registered a callback
    if (TW->user_onReceive)
    {
        // don't bother if rx buffer is in use by a master requestFrom() op
        // i know this drops data, but it allows for slight stupidity
        // meaning, they may not have read all the master requestFrom() data yet
        if (TW->rxBufferIndex >= TW->rxBufferLength)
        {

            TW->allocateRxBuffer(numBytes);
            // error if no memory block available to allocate the buffer
            if (TW->rxBuffer == nullptr)
            {
                Error_Handler();
            }

            // copy twi rx buffer into local read buffer
            // this enables new reads to happen in parallel
            memcpy(TW->rxBuffer, inBytes, numBytes);
            // set rx iterator vars
            TW->rxBufferIndex = 0;
            TW->rxBufferLength = numBytes;
            // alert user program
            TW->user_onReceive(numBytes);
        }
    }
}

TwoWire Wire = TwoWire(); // D14-D15